#ifndef kinova_driver_qt_QNODE_HPP_
#define kinova_driver_qt_QNODE_HPP_

#include <ros/ros.h>
#include <string>
#include <QThread>
#include <QStringListModel>
#include <QUdpSocket>
#include <QXmlStreamReader>
#include "kinovaStatusHandler.hpp"
#include "kinovaController.hpp"
#include "kinova_driver_qt/goto_home_pos.h"
#include "kinova_driver_qt/get_current_state.h"
#include "kinova_driver_qt/moveFingersAction.h"
#include "kinova_driver_qt/moveKinovaAction.h"
#include "kinova_driver_qt/setJointsAction.h"
#include "actionlib/server/simple_action_server.h"
//#include "jointStatePublisher.hpp"
#include <QWaitCondition>
#include "basicUtils.hpp"

namespace kinova_driver_qt {

typedef actionlib::SimpleActionServer<moveKinovaAction>  moveKinova_actionServer;
typedef actionlib::SimpleActionServer<moveFingersAction> moveFingers_actionServer;
typedef actionlib::SimpleActionServer<setJointsAction>   setJoints_actionServer;


class QNode : public QThread
{
    Q_OBJECT
public:
    QNode(int argc, char** argv , int kinova_status_port);
    ~QNode();
    bool init();
    bool kinova_init(ros::NodeHandle nh);
    bool getStatus(Status *status);

    void run();

    enum LogLevel
    {
        Debug,
        Info,
        Warn,
        Error,
        Fatal
    };

    QStringListModel* loggingModel() { return &logging_model; }

    void log( const LogLevel &level, const std::string &msg);

public Q_SLOTS:
    bool reach_cartesianPos( Cartesian cartesian);
    bool reach_angularPos( Joints angular );
    bool set_fingers( Fingers fingers );
    bool gotoHomePos();
    bool emulate_homePos();
    void gotoNavigationPos();
    void ping_kinova();

Q_SIGNALS:
    void loggingUpdated();
    void rosShutdown();
    void kinova_isReady();
    void printLog(QString);

private:

    void emergencyStateCB(const std_msgs::Bool::ConstPtr &msg);

    bool kinova_goto_home_pos_serviceCB( kinova_driver_qt::goto_home_pos::Request &req,
                                         kinova_driver_qt::goto_home_pos::Response &res );
    bool kinova_get_current_state_serviceCB( kinova_driver_qt::get_current_state::Request &req,
                                             kinova_driver_qt::get_current_state::Response &res );

    void kinova_moveKinova_executeCB( const moveKinovaGoalConstPtr &goal);
    void kinova_setJoints_executeCB( const setJointsGoalConstPtr &goal );
    void kinova_moveFingers_executeCB( const moveFingersGoalConstPtr &goal );

    int              init_argc;
    char**           init_argv;
    int              kinovaControllerPort;

    QStringListModel logging_model;

    kinovaStatusHandler     *kinovaStatus_handler;
    kinovaController        *kinova_controller;
//    JointState_publisher    *kinova_jointState_publisher;

    ros::ServiceServer        kinova_goto_homePos_server;
    ros::ServiceServer        kinova_get_current_state_server;

    moveKinova_actionServer  *movekinova_as;
    moveFingers_actionServer *moveFingers_as;
    setJoints_actionServer   *setJoints_as;
    ros::Subscriber          emergency_state;

    QMutex *mutex;
    QWaitCondition *conditionHandler;
    kinova_status *_kinova_status;
};

}  // namespace kinova_driver_qt

#endif /* kinova_driver_qt_QNODE_HPP_ */
